
#include <pic18.h>
#include <delays.h>
#include "my_macros.h"
#include "SysClock.h"
#include "UART.h"
#include "app.h"
#include "String.h"
#include "printf.h"
#include "atd.h"
#include "dbg.h"
//#include "Command.h"
//#include "AppDef.h"

unsigned long SerNum;
unsigned char val_mem;

app_t app;


/* ======================================================= */
#define   VIN_VOL_DIV_R1				470  
#define   VIN_VOL_DIV_R2				51

#define   V_REF				3300000 // uV 
#define   ATD_DIVISION		(V_REF/1024)
#define   mV					1000		//uV
                                            //240768                                        //3222
#define VIN_VOLTS(ROW_DATA) ((((ROW_DATA*(VIN_VOL_DIV_R2+VIN_VOL_DIV_R1))/VIN_VOL_DIV_R2)*ATD_DIVISION)/mV)
/* ======================================================= */

near bit phase_A_monitor_finish;
near bit phase_B_monitor_finish;
near bit phases_connected_OK;

unsigned int phase_B_shift;
signed long phase_A_start_time, phase_B_start_time, time_from_t0;

unsigned long vol_raw;
unsigned long vol_phase_A;
unsigned long vol_phase_B;


#define   GET_TIME_FROM(T0) time_from_t0=TMR1;time_from_t0-=T0;if(time_from_t0<0)time_from_t0+=0xFFFF;		

void phase_A_monitor ( void )
{
	static unsigned char state = STATE_WAIT_FOR_ONE, cnt=0;

	/* dbg("\n\ratd_1_result: %d", atd_1_result); //Dima */
	/* dbg("\n\ratd_2_result: %d", atd_2_result); //Dima */
	
	switch(state)
	{
		case STATE_WAIT_FOR_ONE:
			if ( atd_1_result > 100 )
			{
				if ( ++cnt > 10 )
				{
					state = STATE_VOLTAGE_MEASUREMENT;
					cnt = 0;
				}
			} 
			else
			{
				cnt = 0;
			}
			break;

		case STATE_VOLTAGE_MEASUREMENT:
			GET_TIME_FROM(phase_A_start_time);
			if ( time_from_t0 >= 14500 )
			{
				vol_raw = atd_1_result;
				vol_phase_A = VIN_VOLTS(vol_raw);

				state = STATE_WAIT_FOR_ZERO;
			}
			break;	


		case STATE_WAIT_FOR_ZERO:
			if ( atd_1_result == 0 )
			{
				if ( ++cnt > 3 )
				{
					phase_A_start_time = TMR1; /* from this point count the time for shift checking or phase B */
//					dbg("\n\rphase_A_start_time: %u", phase_A_start_time); //Dima
					state = STATE_WAIT_FOR_ONE;
					cnt = 0;
					phase_A_monitor_finish = TRUE;
				}
			} 
			else
			{
				cnt = 0;
			}
			break;
	}  
}

unsigned int shift;

void phase_B_monitor ( void )
{
	static unsigned char state = STATE_WAIT_FOR_ONE, cnt=0;

//	dbg("\n\r--------"); //Dima
	switch(state)
	{
		case STATE_WAIT_FOR_ONE:
			if ( atd_2_result > 100 )
			{
				if ( ++cnt > 3 )
				{
					state = STATE_VOLTAGE_MEASUREMENT;
					cnt = 0;
				}
			} 
			else
			{
				cnt = 0;
			}
			break;

		case STATE_VOLTAGE_MEASUREMENT:
			GET_TIME_FROM(phase_A_start_time);
			if ( time_from_t0 >= 1300 ) /* 1666uS */
			{
				vol_raw = atd_2_result;
				vol_phase_B = VIN_VOLTS(vol_raw);

				state = STATE_WAIT_FOR_ZERO;
			}
			break;

		case STATE_WAIT_FOR_ZERO:
			if ( atd_2_result == 0 )
			{
				if ( ++cnt > 3 )
				{
					GET_TIME_FROM(phase_A_start_time);
					phase_B_shift = time_from_t0;
					
//					phase_B_start_time = TMR1;
					phase_B_monitor_finish = TRUE;
//					dbg("\n\r......."); //Dima
					state = STATE_WAIT_FOR_ONE;
					cnt = 0;
				}
			} 
			else
			{
				cnt = 0;
			}
			break;
	}  
}

unsigned long vrmsA, vrmsB;

/* phase_B_shift: 5668 */
/* phase_B_shift: 5679 */
/* phase_B_shift: 5674 */
/* phase_B_shift: 5763 */
/* phase_B_shift: 5756 */
/* phase_B_shift: 5840 */
/* phase_B_shift: 5911 */
/* phase_B_shift: 5922 */
/* phase_B_shift: 6007 */
/* phase_B_shift: 5912 */
/* phase_B_shift: 5768 */
/* phase_B_shift: 5775 */
/* phase_B_shift: 5687 */
/* phase_B_shift: 5431 */
/* phase_B_shift: 5352 */
/* phase_B_shift: 5425 */

void app_phase_monitor ( void )
{
	phase_A_monitor();
	
	if ( phase_A_monitor_finish )
	{
		phase_B_monitor();
	
		GET_TIME_FROM(phase_A_start_time);
		

		if ( phase_B_monitor_finish || time_from_t0 > 7000 )
		{
			phase_A_monitor_finish = FALSE;
			phase_B_monitor_finish = FALSE;
		
			//dbg("\n\rphase_B_shift: %l", phase_B_shift); //Dima
			if ( phase_B_shift > 5000 && phase_B_shift < 7000 )
			//if ( phase_B_shift > 3000 && phase_B_shift < 5000 )
			{
				phases_connected_OK = TRUE;
			}
			else
			{
				phases_connected_OK = FALSE;
			}

			shift = phase_B_shift;
			phase_B_shift = 0;
		}
	}

	if ( Time_1000ms )
	{
		vrmsA = vol_phase_A * 707;
		vrmsA /=1000;
		
		vrmsB = vol_phase_B * 707;
		vrmsB /=1000;

		dbg("\n\rphase_B_shift: %d", shift); //Dima
		dbg("\n\rvol phase_A: Amp:%lmV; RMS:%lmV;",     vol_phase_A, vrmsA); //Dima
		dbg("\n\rvol phase_B: Amp:%lmV; RMS:%lmV;", vol_phase_B, vrmsB); //Dima
		
		vol_phase_B = vol_phase_A = vrmsB = vrmsA = 0;
		
		dbg("\n\r ====> Phases connected %s\n\r", phases_connected_OK ? "correct":"NOT correct"); //Dima
	}

	if ( phases_connected_OK  )
	{
		if ( Time_500ms )
		{
			RB3 ^= 1;
			RB5 = 1;
		}
	}
	else
		if ( Time_100ms ) 
		{
			RB3 ^= 1;
			RB5 = 0;
		}
}

void app_init ( void )
{
	TRISB3 = 0;
	printf("\n\r SMARTDIN DEVICE: PHASE DETECTOR");
	app = app_phase_monitor;
	phase_A_monitor_finish = FALSE;
	phase_B_monitor_finish = FALSE;
	phase_B_shift = 0;
	//dbg("\nSerial Number: 0x%X",SerNum ); //Dima
	dbg("\n\rSerial Number: %d",SerNum ); //Dima
}

